clear all;
close all;

J=10;f=6;

Qk=[0;1];
Zk=[0;0];

T=0.001;
for k=1:1:10000
  
    time(k)=k*T;

    thd(k)=sin(k*T);    
    dthd(k)=cos(k*T);   
    th(k)=Qk(1);
    dth(k)=Qk(2);
    Q(1)=th(k);
    Q(2)=dth(k);
    nth(k)=Zk(1);
    ndth(k)=Zk(2);
    

    dt(k)=0.5*sin(2*k*T)+0.5*cos(5*k*T);
    D=dt(k);
    
    U(k)=-30*sign(nth(k)-thd(k))-15*sign(ndth(k)-dthd(k));
    vt=U(k);

    tSpan = [0 T];
	[tt,Qq] = ode45(@(t,x) plant(t,x,vt,D),tSpan,Qk);
    Qk = Qq(length(Qq),:);

    tSpan = [0 T];
	[tt,Zq] = ode45(@(t,x) obv(t,x,vt,Q),tSpan,Zk);
    Zk = Zq(length(Zq),:);

    e(k)=thd(k)-th(k);
    de(k)=dthd(k)-dth(k);
end

figure(1);
plot(time,th,'k',time,thd,'r--');
xlabel('time(s)');ylabel('th');
legend('th','thd');
grid on;

figure(2);
plot(time,dth,'k',time,dthd,'r--');
xlabel('time(s)');ylabel('dth');
legend('dth','dthd');
grid on;

figure(3);
plot(time,U,'k');
xlabel('time(s)');ylabel('U');
legend('U');
grid on;

figure(4);
plot(time,e,'k');
xlabel('time(s)');ylabel('e');
legend('e');
grid on;

figure(5);
plot(time,de,'k');
xlabel('time(s)');ylabel('de');
legend('de');
grid on;
